import os
import json
import queue
from consts import *
import threading

def get_stepper_dict(mst=16, dir_=1, st=1.6, rd=6):
    """细分, 方向, 步进角, 螺距"""
    _dict = {}
    _dict["microsteps"] = mst
    _dict["dir"] = dir_
    _dict["step"] = st
    _dict["rotation_distance"] = rd
    return _dict

def config_init():
    '''配置文件初始化'''
    if os.path.exists("config.json"):
        with open("config.json", "r") as f:
            config:dict = json.load(f)
        for i in range(4):
            if f"stepper_{i}" not in config.keys():
                # 输出轴，输入轴，输入轴范围，输出轴范围
                config[f"stepper_{i}"] = get_stepper_dict()
    else:
        config = {}
        config["stepper_0"] = get_stepper_dict()
        config["stepper_1"] = get_stepper_dict()
        config["stepper_2"] = get_stepper_dict()
        config["stepper_3"] = get_stepper_dict()

    # 同步配置
    with open("config.json", 'w') as js_file:
        json.dump(config, js_file, indent=4)
    return config


class Base_Communicator:
    def write(self, bytes_:bytes)->int:
        print(bytes_)
    def read(self, length:int=None)->bytes:
        ...
    def close(self):
        ...
    def start(self):
        ...


class Robot():
    def __init__(self, communicator:Base_Communicator=None):
        self.config = config_init()
        self.communicator = communicator
        if communicator is not None:
            self.communicator.start()
        else:
            self.communicator = Base_Communicator()
        self.ratios = self.get_convert_ratios()
        self._loop_running = False
        self.lock = threading.Lock()
        self.que = None
        self.guique = None

    def set_gui_signal_que(self, gui_signal:queue.Queue):
        self.guique = gui_signal

    def set_speed(self, id:int, spd:float):
        if self.guique is not None:
            self.guique.put(f"{MOTOR_SPEED} {id} {spd:.2f}")
        self.lock.acquire()
        try:
            self.set_speed_raw(id, spd * self.ratios[id])
        except Exception as e:
            if self.guique is not None:
                self.guique.put(f"{SPD_WRITE_ERROR} {e}")
        finally:
            self.lock.release()

    def set_communicator(self, communicator:Base_Communicator):
        self.lock.acquire()
        if self.communicator is not None:
            self.communicator.close()
        self.communicator = communicator
        self.lock.release()

    def set_speed_raw(self, id:int, freq:float):
        '''设置某个电机轴的速度, 该速度为频率速度, 需要换算后得到'''
        msg = f":{id} {round(freq, 2)}\r\n".encode()
        self.communicator.write(msg)


    def emergency_stop(self):
        self.lock.acquire()
        for i in range(4):
            self.set_speed_raw(i, 0)
        self.lock.release()
    
    def get_convert_ratios(self):
        ratios =[]
        for i in range(4):
            mst, dir_, st, rd = self.config[f"stepper_{i}"].values()
            # 计算 多少脉冲 / mm
            ratio = 1 / (dir_ * rd / ((360 / st) * mst)) # 一圈走多少mm / 每圈有多少脉冲
            ratios.append(ratio)
        return ratios
    
    def main_loop(self, queue:queue.Queue):
        """从一个队列中读取消息, 解析成命令并执行"""
        self._loop_running = True
        self.que =queue
        while self._loop_running:
            if not self.que.empty():
                msg:str = self.que.get()
                # print(msg_lst)
                for line in msg.split("\n"):
                    msg_lst = line.split(" ")
                    if msg_lst[0] == str(MOTOR_SPEED):
                        self.set_speed(int(msg_lst[1]), float(msg_lst[2]))
                    elif msg_lst[0] == str(BUTTON_PUSHED):
                        self.guique.put(msg)
                        if msg_lst[1] == str(3):
                            self.set_speed(2, 10)
                        elif msg_lst[1] == str(0):
                            self.set_speed(2, -10)
                    elif msg_lst[0] == str(BUTTON_RELEASED):
                        self.guique.put(msg)
                        if msg_lst[1] == str(3):
                            self.set_speed(2, 0)
                        elif msg_lst[1] == str(0):
                            self.set_speed(2, 0)
            
            if self.communicator is not None:
                self.lock.acquire()
                try:
                    msg = self.communicator.read()
                except Exception as e:
                    if self.guique is not None:
                        self.guique.put(f"{SERIAL_CANNOT_OPEN} {e}")
                finally:
                    self.lock.release()
                if msg:
                    if self.guique is not None:
                        self.guique.put(f"{GET_MSG} {msg}")


    
if __name__ == "__main__":
    import joystick
    joystick_manager = joystick.Joystick_Manager()
    joystick_manager.start_joystick(0)
    rb = Robot(Base_Communicator())
    rb.main_loop(joystick_manager.joystick_outputs)
